﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Numerics;
using Newtonsoft.Json;
using System.IO;
using NXOpen;
using NXOpen.UF;
using System.Diagnostics;

namespace UGCommon
{
    [Serializable]
    public class TaskPiperouting//管路敷设任务
    {
        private string _task_num;//任务代号 (TEST10)
        private List<group_List> _group_manage;

        public string task_num
        {
            get { return _task_num; }
            set { _task_num = value; }
        }
        public List<group_List> group_manage
        {
            get { return _group_manage; }
            set { _group_manage = value; }
        }
    }
    [Serializable]
    public class group_List//管路分组
    {
        private string _group_num;
        private List<route_list> _pipe_list;//管组代号列表


        public string group_num//管组代号名称
        {
            get { return _group_num; }
            set { _group_num = value; }
        }
        public List<route_list> pipe_list
        {
            get { return _pipe_list; }
            set { _pipe_list = value; }
        }

    }
    [Serializable]
    public class route_list//单管路约束
    {
        private string _pipe_num;//管线任务名称
        private double _pipediameter;//管路外径
        private string _portType;//介质类别（燃油）
        private double[] _startpoint;
        private double[] _endpoint;
        private double[] _startvector;
        private double[] _endvector;
        private List<route_constraint> _constraint_list;
        private List<double[]> _waypoint;
        public string pipe_num
        {
            get { return _pipe_num; }
            set { _pipe_num = value; }
        }
        public double pipediameter
        {
            get { return _pipediameter; }
            set { _pipediameter = value; }
        }
        public double[] startpoint
        {
            get { return _startpoint; }
            set { _startpoint = value; }
        }
        public double[] endpoint
        {
            get { return _endpoint; }
            set { _endpoint = value; }
        }
        public double[] startvector
        {
            get { return _startvector; }
            set { _startvector = value; }
        }

        public double[] endvector
        {
            get { return _endvector; }
            set { _endvector = value; }
        }
        public string portType
        {
            get { return _portType; }
            set { _portType = value; }
        }
        public List<route_constraint> constraint_list
        {
            get { return _constraint_list; }
            set { _constraint_list = value; }
        }
        public List<double[]> waypoint
        {
            get { return _waypoint; }
            set { _waypoint = value; }
        }
    }
    [Serializable]
    public class route_constraint//约束
    {
        private string _name;
        private double[] _contralPoints;
        private double[] _middlePoints;
        private double[] _vector;
        private List<double[]> _extensionPoints;
        public string name
        {
            get { return _name; }
            set { _name = value; }
        }

        public double[] contralPoints
        {
            get { return _contralPoints; }
            set { _contralPoints = value; }
        }
        public double[] middlePoints
        {
            get { return _middlePoints; }
            set { _middlePoints = value; }
        }
        public double[] vector
        {
            get { return _vector; }
            set { _vector = value; }
        }
        public List<double[]> extensionPoints
        {
            get { return _extensionPoints; }
            set { _extensionPoints = value; }
        }
    }

    [Serializable]
    public class components
    {
        private string _name;
        private List<component> _component;
        private List<component> _kafcomponent;
        private List<pipeS> _pipeSlist;
        private List<Generatrix> _generatrixs;
        private pipeS _casing;
        // 添加构造函数以初始化_boxlist
        public components()
        {
            component = new List<component>(); // 确保总是有一个默认的空列表实例
            pipeSlist = new List<pipeS>();
            generatrixs = new List<Generatrix>();
        }
        public List<component> kafcomponent
        {
            get { return _kafcomponent; }
            set { _kafcomponent = value; }
        }
        public void createj2py(string path = @"D:\\cmps.json")
        {
            string jsonString = JsonConvert.SerializeObject(this);
            string filePath = path;
            File.WriteAllText(filePath, jsonString);
        }
        public void kaf(List<double[]> clampmiddlePoints)
        {
            kafcomponent = new List<component>();
            if (clampmiddlePoints.Count==0)
            {
                kafcomponent = component;
                return ;
            }
            foreach (var m in this.component)
            {
                int mask = 1;
                if (m.name != "")
                {
                    component mm = new component();
                    mm.name = m.name;
                    foreach (box n in m.boxlist)
                    {
                        foreach(var clamp in clampmiddlePoints)
                        {
                            if (UGMath.getTwoPointDistance(n.centerPoint, clamp) < 50)
                            {
                                foreach (double[] kaf in n.coordpoints)
                                {
                                    if (UGMath.getTwoPointDistance(kaf, clamp) < 50)
                                    {
                                        mask = -1;
                                    }
                                } 
                            }
                        }
                        if (mask == 1)
                        {
                            mm.boxlist.Add(n);
                        }
                    }
                    kafcomponent.Add(mm);
                }
            }
            return ;
        }
        public string name
        {
            get { return _name; }
            set { _name = value; }
        }
        public pipeS casing
        {
            get { return _casing; }
            set { _casing = value; }
        }
        public List<component> component
        {
            get { return _component; }
            set { _component = value; }
        }
       
        public List<pipeS> pipeSlist
        {
            get { return _pipeSlist; }
            set { _pipeSlist = value; }
        }
        public List<Generatrix> generatrixs
        {
            get { return _generatrixs; }
            set { _generatrixs = value; }
        }
      

    }

    [Serializable]
   public class Generatrix
    {
        private double _x;
        private double _d;


        public double x
        {
            get { return _x; }
            set { _x = value; }
        }
     
        public double d
        {
            get { return _d; }
            set { _d = value; }
        }
      

    }
    [Serializable]
    public class pipeS
    {
        private double[] _p1;
        private double[] _p2;
        private double _d;

        public double[] p1
        {
            get { return _p1; }
            set { _p1 = value; }
        }
        public double[] p2
        {
            get { return _p2; }
            set { _p2 = value; }
        }
        public double d
        {
            get { return _d; }
            set { _d = value; }
        }
    }

    public class test1
    {
        public components LoadComponentsDataFromFile(string filePath)
        {
            components components = new components { name = "Piping System" };

            using (StreamReader reader = new StreamReader(filePath))
            {
                string line;
                while ((line = reader.ReadLine()) != null)
                {

                    string[] values = line.Split(' ');
                    //foreach (string value in values)
                    //{
                    //    UGTools.writeInListWindow(value); // 假设这个方法用于输出或处理单个值
                    //}

                    double[] p1 = { double.Parse(values[0]), double.Parse(values[1]), double.Parse(values[2]) };
                    double[] p2 = { double.Parse(values[3]), double.Parse(values[4]), double.Parse(values[5]) };
                    double[] p3 = { double.Parse(values[6]), double.Parse(values[7]), double.Parse(values[8]) };
                    double[] p4 = { double.Parse(values[9]), double.Parse(values[10]), double.Parse(values[11]) };
                    double diameter = 9;

                    pipeS pipeS1 = new pipeS { p1 = p1, p2 = p2, d = diameter };
                    pipeS pipeS2 = new pipeS { p1 = p3, p2 = p4, d = diameter };
                    PSI psi = new PSI(pipeS1, pipeS2);
                    double[] ruslt = psi.robust_intersection();
                    if (ruslt[2] - double.Parse(values[12]) < 5e-8)
                    {
                        //UGTools.writeInListWindow("0");
                    }
                    else
                    {
                        UGTools.writeInListWindow("错误" + (ruslt[2] - double.Parse(values[12])).ToString() + "||" + ruslt[2].ToString() + "||" + values[12]);
                    }

                }
            }

            return components;
        }

    }

    //鲁棒计算线段与线段干涉和最近距离，返回参数方程2个参数，距离，干涉与否。
    public class PSI
    {
        private double _psi_threshold { get; set; }//计算精度
        private pipeS _segment1 { get; set; }
        private pipeS _segment2 { get; set; }
        private double[] _P0 { get; set; }
        private double[] _P1 { get; set; }
        private double[] _Q0 { get; set; }
        private double[] _Q1 { get; set; }
        private double _d1 { get; set; }
        private double _d2 { get; set; }
        private double a { get; set; }
        private double b { get; set; }
        private double c { get; set; }
        private double d { get; set; }
        private double e { get; set; }
        private double f { get; set; }


        public PSI(pipeS segment1, pipeS segment2, double threshold = 0.001)
        {
            _psi_threshold = threshold;
            _segment1 = segment1;
            _segment2 = segment2;
            _P0 = segment1.p1;
            _P1 = segment1.p2;
            _Q0 = segment2.p1;
            _Q1 = segment2.p2;
            _d1 = segment1.d;
            _d2 = segment2.d;
            a = Dot(Sub(_P1, _P0), Sub(_P1, _P0));
            b = Dot(Sub(_P1, _P0), Sub(_Q1, _Q0));
            c = Dot(Sub(_Q1, _Q0), Sub(_Q1, _Q0));
            d = Dot(Sub(_P1, _P0), Sub(_P0, _Q0));
            e = Dot(Sub(_Q1, _Q0), Sub(_P0, _Q0));
            f = Dot(Sub(_P0, _Q0), Sub(_P0, _Q0));
        }
        public double[] Sub(double[] p, double[] q)
        {
            if (p.Length != q.Length)
            {
                throw new ArgumentException("向量维度不对");
            }

            double[] result = new double[p.Length];
            for (int i = 0; i < p.Length; i++)
            {
                result[i] = p[i] - q[i];
            }
            return result;
        }

        public double Dot(double[] p, double[] q)
        {
            if (p.Length != q.Length)
            {
                throw new ArgumentException("向量维度不对");
            }

            return p.Zip(q, (x, y) => x * y).Sum();
        }

        public double GetClampedRoot(double real_sigma, double real_h0, double real_h1)
        {
            double root = 0;
            if (real_h0 < 0)
            {
                if (real_h1 > 0)
                {
                    root = -real_h0 / real_sigma;
                    if (root > 1)
                    {
                        root = 0.5;
                    }
                }
                else
                {
                    root = 1;
                }

            }
            else
            {
                root = 0;
            }
            return root;
        }

        public void ComputeIntersection(double[] hatS, int[] classify, double b, double f00, double f10, out int[] edge, out double[,] end)
        {
            int zero = 0;
            double half = 0.5;
            int one = 1;

            edge = new int[2];
            end = new double[2, 2];

            if (classify[0] < 0)
            {
                edge[0] = 0;
                end[0, 0] = zero;
                end[0, 1] = f00 / b;
                if (end[0, 1] < zero || end[0, 1] > one)
                {
                    end[0, 1] = half;
                }

                if (classify[1] == 0)
                {
                    edge[1] = 3;
                    end[1, 0] = hatS[1];
                    end[1, 1] = one;
                }
                else // classify[1] > 0
                {
                    edge[1] = 1;
                    end[1, 0] = one;
                    end[1, 1] = f10 / b;
                    if (end[1, 1] < zero || end[1, 1] > one)
                    {
                        end[1, 1] = half;
                    }
                }
            }
            else if (classify[0] == 0)
            {
                edge[0] = 2;
                end[0, 0] = hatS[0];
                end[0, 1] = zero;
                if (classify[1] < 0)
                {
                    edge[1] = 0;
                    end[1, 0] = zero;
                    end[1, 1] = f00 / b;
                    if (end[1, 1] < zero || end[1, 1] > one)
                    {
                        end[1, 1] = half;
                    }
                }
                else if (classify[1] == 0)
                {
                    edge[1] = 3;
                    end[1, 0] = hatS[1];
                    end[1, 1] = one;
                }
                else
                {
                    edge[1] = 1;
                    end[1, 0] = one;
                    end[1, 1] = f10 / b;
                    if (end[1, 1] < zero || end[1, 1] > one)
                    {
                        end[1, 1] = half;
                    }
                }
            }
            else
            {
                edge[0] = 1;
                end[0, 0] = one;
                end[0, 1] = f10 / b;
                if (end[0, 1] < zero || end[0, 1] > one)
                {
                    end[0, 1] = half;
                }
                if (classify[1] == 0)
                {
                    edge[1] = 3;
                    end[1, 0] = hatS[1];
                    end[1, 1] = one;
                }
                else
                {
                    edge[1] = 0;
                    end[1, 0] = zero;
                    end[1, 1] = f00 / b;
                    if (end[1, 1] < zero || end[1, 1] > one)
                    {
                        end[1, 1] = half;
                    }
                }
            }


        }
        public double[] ComputeMinimumParameters(int[] edge, double[,] end, double b, double c, double e, double g00, double g10, double g01, double g11)
        {
            double[] parameter = new double[2];
            double zero = 0;
            double one = 1;
            double delta = end[1, 1] - end[0, 1];
            double h0 = delta * (-b * end[0, 0] + c * end[0, 1] - e);

            if (h0 >= zero)
            {
                if (edge[0] == 0)
                {
                    parameter[0] = zero;
                    parameter[1] = GetClampedRoot(c, g00, g01);
                }
                else if (edge[0] == 1)
                {
                    parameter[0] = one;
                    parameter[1] = GetClampedRoot(c, g10, g11);
                }
                else
                {
                    parameter[0] = end[0, 0];
                    parameter[1] = end[0, 1];
                }
            }
            else
            {
                double h1 = delta * (-b * end[1, 0] + c * end[1, 1] - e);
                if (h1 <= zero)
                {
                    if (edge[1] == 0)
                    {
                        parameter[0] = zero;
                        parameter[1] = GetClampedRoot(c, g00, g01);
                    }
                    else if (edge[1] == 1)
                    {
                        parameter[0] = one;
                        parameter[1] = GetClampedRoot(c, g10, g11);
                    }
                    else
                    {
                        parameter[0] = end[1, 0];
                        parameter[1] = end[1, 1];
                    }
                }
                else
                {
                    double z = Math.Min(Math.Max(h0 / (h0 - h1), zero), one);
                    double omz = one - z;
                    parameter[0] = omz * end[0, 0] + z * end[1, 0];
                    parameter[1] = omz * end[0, 1] + z * end[1, 1];
                }
            }
            return parameter;
        }
        public double[] robust_intersection()
        {
            double f00 = d;
            double f10 = d + a;
            double f01 = d - b;
            double f11 = d + a - b;

            double g00 = -e;
            double g10 = -e - b;
            double g01 = -e + c;
            double g11 = -e - b + c;

            double[] parameter = new double[2] { 0, 0 };

            if (a > 0 && c > 0)
            {
                double[] hatS = new double[2] { 0, 0 };
                hatS[0] = GetClampedRoot(a, f00, f10);
                hatS[1] = GetClampedRoot(a, f01, f11);

                int[] classify = new int[2] { 0, 1 };

                for (int i = 0; i < 2; i++)
                {
                    if (hatS[i] <= 0)
                    {
                        classify[i] = -1;
                    }
                    else if (hatS[i] >= 1)
                    {
                        classify[i] = 1;
                    }
                    else
                    {
                        classify[i] = 0;
                    }
                }


                if (classify[0] == -1 && classify[1] == -1)
                {
                    parameter[0] = 0;
                    parameter[1] = GetClampedRoot(c, g00, g01);
                }
                else if (classify[0] == 1 && classify[1] == 1)
                {
                    parameter[0] = 1;
                    parameter[1] = GetClampedRoot(c, g10, g11);
                }
                else
                {
                    int[] edge = new int[2] { 0, 0 };
                    double[,] end = new double[2, 2];

                    ComputeIntersection(hatS, classify, b, f00, f10, out edge, out end);
                    parameter = ComputeMinimumParameters(edge, end, b, c, e, g00, g10, g01, g11);

                }
            }
            else
            {
                if (a > 0)
                {
                    parameter[0] = GetClampedRoot(a, f00, f10);
                    parameter[1] = 0;
                }
                else if (c > 0)
                {
                    parameter[0] = 0;
                    parameter[1] = GetClampedRoot(c, g00, g01);
                }
                else
                {
                    parameter[0] = 0;
                    parameter[1] = 0;
                }

            }
            double[,] closest = new double[2, 3];

            // 对于closest的第一行
            closest[0, 0] = (1 - parameter[0]) * _P0[0] + parameter[0] * _P1[0];
            closest[0, 1] = (1 - parameter[0]) * _P0[1] + parameter[0] * _P1[1];
            closest[0, 2] = (1 - parameter[0]) * _P0[2] + parameter[0] * _P1[2];

            // 对于closest的第二行
            closest[1, 0] = (1 - parameter[1]) * _Q0[0] + parameter[1] * _Q1[0];
            closest[1, 1] = (1 - parameter[1]) * _Q0[1] + parameter[1] * _Q1[1];
            closest[1, 2] = (1 - parameter[1]) * _Q0[2] + parameter[1] * _Q1[2];

            // 假设closest是一个二维数组，需要先将差值diff计算出来
            double[] diff = new double[3];
            for (int i = 0; i < 3; i++)
            {
                diff[i] = closest[0, i] - closest[1, i];
            }

            // 计算距离，使用System.Math类中的静态方法
            double distance = Math.Sqrt(diff.Select(x => x * x).Sum());
            int itf = 0;
            if (distance > (_d1 + _d2))
            {
                itf = 0;
            }
            else
            {
                itf = 1;
            }
            double[] ruslt = new double[4];
            ruslt[0] = parameter[0];
            ruslt[1] = parameter[1];
            ruslt[2] = distance;
            ruslt[3] = itf;
            return ruslt;
        }
        // 新增静态方法实现线性插值计算
        public static double Interpolate(List<Generatrix> generatrixs, double targetX)
        {
            if (generatrixs == null || generatrixs.Count < 2)
                throw new ArgumentException("The list of generatrix points must contain at least two elements.");

            // 确保列表按x值升序排列，如果外部不保证，则需要在此处排序
            // generatrixs.Sort((a, b) => a.X.CompareTo(b.X));

            // 优化查找目标x值插入位置的逻辑
            int index = generatrixs.FindIndex(g => g.x >= targetX);

            //UGTools.writeInListWindow("index1"+index.ToString());
            if (index == 0)
            {
                // 目标x小于或等于列表中的最小x值
                index = 0;
                //UGTools.writeInListWindow("index2" + index.ToString());
            }
            else if (index == generatrixs.Count||( index ==-1))
            { // 目标x大于列表中的最大x值
                index = generatrixs.Count - 1;
                //UGTools.writeInListWindow("index3" + index.ToString());
            }
            else
            {// 目标x值在两个点之间
                index--; // 移动到前一个点，以便进行插值
                //UGTools.writeInListWindow("index4" + index.ToString());
            }

            var pointBefore = generatrixs[index];

            // 确保pointAfter不会越界
            var pointAfter = index + 1 < generatrixs.Count ? generatrixs[index + 1] : pointBefore;

            // 计算线性插值
            double interpolatedD = pointBefore.d + (targetX - pointBefore.x) * ((pointAfter.d - pointBefore.d) / (pointAfter.x - pointBefore.x));
            return interpolatedD;


        }
    }


    [Serializable]
    public class component
    {
        private string _name;
        private List<box> _boxlist;
        public component()
        {
            boxlist = new List<box>();
        }
        public string name
        {
            get { return _name; }
            set { _name = value; }
        }
        public List<box> boxlist
        {
            get { return _boxlist; }
            set { _boxlist = value; }
        }
    }
    [Serializable]
    public class box
    {
        private string _name;
        private double[] _centerPoint;//中心点
        private List<double[]> _coordpoints;//x,y,z形式
        private Matrix3 _csy;//坐标系
        public string name
        {
            get { return _name; }
            set { _name = value; }
        }
        public double[] centerPoint
        {
            get { return _centerPoint; }
            set { _centerPoint = value; }
        }
        public List<double[]> coordpoints
        {
            get { return _coordpoints; }
            set { _coordpoints = value; }
        }
        public Matrix3 csy
        {
            get { return _csy; }
            set { _csy = value; }
        }
    }
    public class DataContainer
    {
        public double[] ResolutionRatio { get; set; }
        public double[] MinPoint { get; set; }
        public double[] MaxPoint { get; set; }
        public double[] CenterPoint { get; set; }
        public string Path { get; set; }
        public List<double[]> Port1Center { get; set; }
        public List<double[]> Port2Center { get; set; }
        public List<double[]> Port1Norm { get; set; }
        public List<double[]> Port2Norm { get; set; }
        public double[,,] Old_Grid_Map { get; set; }
    }
    public class old_gridmap
    {
        public List<double> resolution_ratio { get; set; }
        public List<double> minpoint { get; set; }
        public List<double> maxpoint { get; set; }
        public List<double> centerpoint { get; set; }
        public List<List<List<double>>> grid { get; set; }
        public double[,,] old_grid { get; set; }

        public old_gridmap getold_map(string path)
        {
            // 读取JSON文件
            string jsonString = File.ReadAllText(path);

            // 反序列化JSON字符串到MyData对象
            old_gridmap data = JsonConvert.DeserializeObject<old_gridmap>(jsonString);


            // 将Grid转换为double[,,]数组
            data.old_grid = new double[data.grid.Count, data.grid[0].Count, data.grid[0][0].Count];
            for (int i = 0; i < data.grid.Count; i++)
            {
                for (int j = 0; j < data.grid[i].Count; j++)
                {
                    for (int k = 0; k < data.grid[i][j].Count; k++)
                    {
                        data.old_grid[i, j, k] = data.grid[i][j][k];
                    }
                }
            }
            return data;
        }
        public double[] grid2coord(double[] x)
        {
            double[] coord = new double[] { 0, 0, 0 };
            return coord;
        }

        public string  runpy(string path,string path2,double[] resolution_ratio,
            double[]minpoint,double[]maxpoint,double[] centerpoint)
        {
            string exePath = path;
            string output="";
            var data = new DataContainer
            {
                ResolutionRatio = resolution_ratio,
                MinPoint = minpoint,
                MaxPoint = maxpoint,
                CenterPoint = centerpoint,
                Path = path2
            };
            string jsonString = JsonConvert.SerializeObject(data);
            using (StreamWriter file = File.CreateText(@"D:\\606ApplicationV1.0\\run\\config\\data.json"))
            {
                file.Write(jsonString);
            }
            //UGTools.writeInListWindow("1");
            ProcessStartInfo startInfo = new ProcessStartInfo();
            startInfo.FileName = exePath;
           // startInfo.Arguments = arguments;
            startInfo.UseShellExecute = false; // 不使用操作系统外壳来启动
            startInfo.RedirectStandardOutput = true; // 重定向输出流
            startInfo.CreateNoWindow = true; // 不创建新窗口

            UGTools.writeInListWindow(startInfo.FileName);
            using (Process process = Process.Start(startInfo))
            {
                if (process != null)
                {
                    // 如果需要读取输出，可以这样：
                    output = process.StandardOutput.ReadToEnd();
                   // UGTools.writeInListWindow("Output: " + output);
                    process.WaitForExit(); // 等待进程结束
                    int exitCode = process.ExitCode; // 获取退出码

                }
            }
            return output;
        }

    }
    public class PipeRoute
    {
        [JsonProperty("管路名称")]
        public string PipeName { get; set; }

        [JsonProperty("管路适应度")]
        public double Fitness { get; set; }

        [JsonProperty("管路中心节点")]
        public List<List<double>> CenterNodes { get; set; }
    }

    public class PipeRoutes
    {
        [JsonProperty("管路顺序")]
        public List<PipeRoute> Routes { get; set; }
    }
    public class JudgeResult
    {
        public void showbox(components cmps)
        {
            List<component> cp = cmps.kafcomponent;
            foreach (component m in cp)
            {
                foreach (box n in m.boxlist)
                {
                    DisplayBoundingBox(n.coordpoints);
                }

            }
        }
        public bool Judge(List<pipeS> ps, components cmps)
        {
            //Session theSession = Session.GetSession();
            //UFSession theUFSession = UFSession.GetUFSession();

            //Part workPart = theSession.Parts.Work;
            List<pipeS> ps2 = cmps.pipeSlist;
            List<component> cp = cmps.component;
            //UGTools.writeInListWindow(ps2.Count.ToString());
            foreach (pipeS i in ps)
            {
                PSI psi = new PSI(i, cmps.casing);
                double[] rus = psi.robust_intersection();
                if (rus[3] == 1)
                {
                    List<double[]> sortedPoints = new List<double[]>();
                    double[] p1 = new double[2] { i.p1[0], Math.Sqrt(i.p1[1] * i.p1[1] + i.p1[2] * i.p1[2]) };
                    double[] p2 = new double[2] { i.p2[0], Math.Sqrt(i.p2[1] * i.p2[1] + i.p2[2] * i.p2[2]) };

                    sortedPoints.Add(p1);
                    sortedPoints.Add(p2);

                    if (0<rus[0]&& rus[0] < 1)
                    {
                        double[] closest = new double[3];
                        closest[0] = (1 - rus[0]) * i.p1[0] + rus[0] * i.p2[0];
                        closest[1] = (1 - rus[0]) * i.p1[1] + rus[0] * i.p2[1];
                        closest[2] = (1 - rus[0]) * i.p1[2] + rus[0] * i.p2[2];
                        
                       
                        double[] p3 = new double[2] { closest[0], Math.Sqrt(closest[1] * closest[1] + closest[2] * closest[2]) };

                        sortedPoints.Add(p3);
                    }

                    // 使用LINQ进行排序
                    sortedPoints = sortedPoints.OrderBy(point => point[0]).ToList();
                    int lastm =0;
                    double mas = PSI.Interpolate(cmps.generatrixs, sortedPoints[0][0]);
                    if (mas < sortedPoints[0][1])
                    {
                        lastm = 1;
                    }
                    else if (mas== sortedPoints[0][1])
                    {
                        lastm = 0;
                        return true;
                    }
                    else
                    {
                        lastm = -1;
                    }
                    foreach (var m  in sortedPoints)
                    {
                        int mmm = 0;
                        double masws = PSI.Interpolate(cmps.generatrixs, m[0]);
                        if (masws < m[1])
                        {
                            mmm = 1;
                         }
                        else if (masws == m[1])
                        {
                            mmm = 0;
                        }
                        else
                        {
                            mmm = -1;
                        }
                        if (mmm * lastm <= 0)
                        {
                            return true;
                        }

                    }

                    //UGTools.writeInListWindow("0");
                    //return true;
                }


                foreach (pipeS j in ps2)
                {
                    psi = new PSI(i, j);
                    double[] ruslt = psi.robust_intersection();
                    if (ruslt[3] == 1)
                    {
                        //UGTools.writeInListWindow("0");
                        return true;
                    }
                }
                PBI pbi = new PBI();
                foreach (component m in cp)
                {
                    if (m.name != "")
                    {
                        //UGTools.writeInListWindow(m.name.ToString());
                        foreach (box n in m.boxlist)
                        {
                            if (pbi.pbi(n, i))
                            {
                               
                                //UGTools.writeInListWindow("1");
                                return true;
                            }
                            else
                            {
                              
                            }
                        }
                    }
                    else
                    {
                        //UGTools.writeInListWindow(m.name.ToString());
                    }
                }
                //Point3d startPoint = UGMath.convertArrayToPoint3d(i.p1);
                //Point3d endPoint = UGMath.convertArrayToPoint3d(i.p2);
                //Line lineData1 = workPart.Curves.CreateLine(startPoint, endPoint);
                //lineData1.SetVisibility(SmartObject.VisibilityOption.Visible);
                //theUFSession.Obj.SetColor(lineData1.Tag, 186);

            }
            return false;
        }
        public int Judge2(List<pipeS> ps, components cmps)
        {
            Session theSession = Session.GetSession();
            UFSession theUFSession = UFSession.GetUFSession();
            int jud1 = 0;
            Part workPart = theSession.Parts.Work;
            List<pipeS> ps2 = cmps.pipeSlist;
            List<component> cp = cmps.component;
            //List<component> cp = cmps.kafcomponent;
            //UGTools.writeInListWindow(ps2.Count.ToString());
            foreach (pipeS i in ps)
            {
                int jud = 0;
                PSI psi = new PSI(i, cmps.casing);
                //Point3d startPoint1 = UGMath.convertArrayToPoint3d(cmps.casing.p1);
                //Point3d endPoint1 = UGMath.convertArrayToPoint3d(cmps.casing.p2);
                //Line lineData11 = workPart.Curves.CreateLine(startPoint1, endPoint1);
                //lineData11.SetVisibility(SmartObject.VisibilityOption.Visible);
                //theUFSession.Obj.SetColor(lineData11.Tag, 186);

                double[] rus = psi.robust_intersection();
                if (rus[3] == 1)
                {
                    List<double[]> sortedPoints = new List<double[]>();
                    double[] p1 = new double[2] { i.p1[0], Math.Sqrt(i.p1[1] * i.p1[1] + i.p1[2] * i.p1[2]) };
                    double[] p2 = new double[2] { i.p2[0], Math.Sqrt(i.p2[1] * i.p2[1] + i.p2[2] * i.p2[2]) };

                    sortedPoints.Add(p1);
                    sortedPoints.Add(p2);

                    if (0 <= rus[0] && rus[0] <= 1)
                    {
                        double[] closest = new double[3];
                        closest[0] = (1 - rus[0]) * i.p1[0] + rus[0] * i.p2[0];
                        closest[1] = (1 - rus[0]) * i.p1[1] + rus[0] * i.p2[1];
                        closest[2] = (1 - rus[0]) * i.p1[2] + rus[0] * i.p2[2];

                        double[] p3 = new double[2] { closest[0], Math.Sqrt(closest[1] * closest[1] + closest[2] * closest[2]) };

                        sortedPoints.Add(p3);
                    }

                    // 使用LINQ进行排序
                    sortedPoints = sortedPoints.OrderBy(point => point[0]).ToList();
                    foreach (var m in sortedPoints)
                    {
                        double masws = PSI.Interpolate(cmps.generatrixs, m[0])+i.d;
                        if (masws < m[1])
                        {
                        }
                        else if (masws == m[1])
                        {
                            jud += 5;
                            break;
                        }
                        else
                        {
                            jud += 5;
                            break;
                        }
                    }            
                }
                else
                {
                }
                if (jud != 0)
                {
                    jud1 += jud;
                    continue;
                }
                else
                {
                    //Point3d startPoint = UGMath.convertArrayToPoint3d(i.p1);
                    //Point3d endPoint = UGMath.convertArrayToPoint3d(i.p2);
                    //Line lineData1 = workPart.Curves.CreateLine(startPoint, endPoint);
                    //lineData1.SetVisibility(SmartObject.VisibilityOption.Visible);
                    //theUFSession.Obj.SetColor(lineData1.Tag, 145);
                }
                jud = 0;
                foreach (pipeS j in ps2)
                {
                    psi = new PSI(i, j);
                    double[] ruslt = psi.robust_intersection();
                    if (ruslt[3] == 1)
                    {
                        //UGTools.writeInListWindow("0");
                        jud++;
                        break;
                        
                    }
                }
                if (jud != 0)
                {
                    jud1 += jud;
                    continue;
                }
                PBI pbi = new PBI();
                jud = 0;
                foreach (component m in cp)
                {
                    if (m.name != "")
                    {
                        //UGTools.writeInListWindow(i.d.ToString());
                        foreach (box n in m.boxlist)
                        {
                            if (pbi.pbi(n, i))
                            {
                               
                                //UGTools.writeInListWindow("1");
                                jud++;
                               
                                //DisplayBoundingBox(n.coordpoints);
                                //UGTools.ShowMessage("1");
                                //UGTools.UnhighlightAll();
                                break;
                            }
                            else
                            {
                            }
                        }
                    }
                }
                if (jud != 0)
                {
                    jud1 += jud;
                    continue;
                }
                else
                {
                    //Point3d startPoint = UGMath.convertArrayToPoint3d(i.p1);
                    //Point3d endPoint = UGMath.convertArrayToPoint3d(i.p2);
                    //Line lineData1 = workPart.Curves.CreateLine(startPoint, endPoint);
                    //lineData1.SetVisibility(SmartObject.VisibilityOption.Visible);
                    //theUFSession.Obj.SetColor(lineData1.Tag, 145);
                }
            
            }
           
            return jud1;
        }
        public int Judge3(List<pipeS> ps, components cmps)
        {
            Session theSession = Session.GetSession();
            UFSession theUFSession = UFSession.GetUFSession();
            int jud1 = 0;
            Part workPart = theSession.Parts.Work;
            List<pipeS> ps2 = cmps.pipeSlist;
            List<component> cp = cmps.component;
            //List<component> cp = cmps.kafcomponent;
            //UGTools.writeInListWindow(ps2.Count.ToString());
            foreach (pipeS i in ps)
            {
                int jud = 0;
                PSI psi = new PSI(i, cmps.casing);

                double[] rus = psi.robust_intersection();
                if (rus[3] == 1)
                {
                    List<double[]> sortedPoints = new List<double[]>();
                    double[] p1 = new double[2] { i.p1[0], Math.Sqrt(i.p1[1] * i.p1[1] + i.p1[2] * i.p1[2]) };
                    double[] p2 = new double[2] { i.p2[0], Math.Sqrt(i.p2[1] * i.p2[1] + i.p2[2] * i.p2[2]) };

                    sortedPoints.Add(p1);
                    sortedPoints.Add(p2);

                    if (0 <= rus[0] && rus[0] <= 1)
                    {
                        double[] closest = new double[3];
                        closest[0] = (1 - rus[0]) * i.p1[0] + rus[0] * i.p2[0];
                        closest[1] = (1 - rus[0]) * i.p1[1] + rus[0] * i.p2[1];
                        closest[2] = (1 - rus[0]) * i.p1[2] + rus[0] * i.p2[2];

                        double[] p3 = new double[2] { closest[0], Math.Sqrt(closest[1] * closest[1] + closest[2] * closest[2]) };

                        sortedPoints.Add(p3);
                    }

                    // 使用LINQ进行排序
                    sortedPoints = sortedPoints.OrderBy(point => point[0]).ToList();
                    foreach (var m in sortedPoints)
                    {
                        double masws = PSI.Interpolate(cmps.generatrixs, m[0]) + i.d;
                        if (masws < m[1])
                        {

                        }
                        else if (masws == m[1])
                        {
                            jud = 5;
                            break;
                        }
                        else
                        {
                            jud = 5;
                            break;
                        }
                    }
                }
                else
                {
                }
                if (jud != 0)
                {
                    jud1 += jud;
                    continue;
                }
                else
                {
                    //Point3d startPoint = UGMath.convertArrayToPoint3d(i.p1);
                    //Point3d endPoint = UGMath.convertArrayToPoint3d(i.p2);
                    //Line lineData1 = workPart.Curves.CreateLine(startPoint, endPoint);
                    //lineData1.SetVisibility(SmartObject.VisibilityOption.Visible);
                    //theUFSession.Obj.SetColor(lineData1.Tag, 145);
                }
                jud = 0;
                foreach (pipeS j in ps2)
                {
                    psi = new PSI(i, j);
                    double[] ruslt = psi.robust_intersection();
                    if (ruslt[3] == 1)
                    {
                        //UGTools.writeInListWindow("0");
                        jud=1;
                        break;
                    }
                }
                if (jud != 0)
                {
                    jud1 += jud;
                    continue;
                }
                PBI pbi = new PBI();
                jud = 0;
                foreach (component m in cp)
                {
                    if (m.name != "")
                    {
                        //UGTools.writeInListWindow(i.d.ToString());
                        foreach (box n in m.boxlist)
                        {
                            if (pbi.pbi(n, i))
                            {

                                UGTools.writeInListWindow("111");
                                jud =1;
                                UGTools.writeInListWindow(m.name);
                                DisplayBoundingBox(n.coordpoints);
                                UGTools.writeInListWindow("||"+n.coordpoints[0][0].ToString()+ "||" + n.coordpoints[0][1].ToString()+ "||" + n.coordpoints[0][2].ToString());
                                UGTools.writeInListWindow("||" + n.coordpoints[1][0].ToString() + "||" + n.coordpoints[1][1].ToString() + "||" + n.coordpoints[1][2].ToString());
                                UGTools.writeInListWindow("||" + n.coordpoints[2][0].ToString() + "||" + n.coordpoints[2][1].ToString() + "||" + n.coordpoints[2][2].ToString());
                                UGTools.writeInListWindow("||" + n.coordpoints[3][0].ToString() + "||" + n.coordpoints[3][1].ToString() + "||" + n.coordpoints[3][2].ToString());
                                UGTools.writeInListWindow("||" + n.coordpoints[4][0].ToString() + "||" + n.coordpoints[4][1].ToString() + "||" + n.coordpoints[4][2].ToString());
                                UGTools.writeInListWindow("||" + n.coordpoints[5][0].ToString() + "||" + n.coordpoints[5][1].ToString() + "||" + n.coordpoints[5][2].ToString());
                                UGTools.writeInListWindow("||" + n.coordpoints[6][0].ToString() + "||" + n.coordpoints[6][1].ToString() + "||" + n.coordpoints[6][2].ToString());
                                UGTools.writeInListWindow("||" + n.coordpoints[7][0].ToString() + "||" + n.coordpoints[7][1].ToString() + "||" + n.coordpoints[7][2].ToString());
                    
                                //UGTools.ShowMessage("1");
                                //UGTools.UnhighlightAll();
                                break;
                            }
                            else
                            {
                            }
                        }
                    }
                }
                if (jud != 0)
                {
                    Point3d startPoint = UGMath.convertArrayToPoint3d(i.p1);
                    Point3d endPoint = UGMath.convertArrayToPoint3d(i.p2);
                    Line lineData1 = workPart.Curves.CreateLine(startPoint, endPoint);
                    lineData1.SetVisibility(SmartObject.VisibilityOption.Visible);
                    theUFSession.Obj.SetColor(lineData1.Tag, 145);
                    jud1 += jud;
                    continue;
                }
                else
                {
                    
                }

            }

            return jud1;
        }
        private double[] convertCoordToPolar(double x, double y, double z)
        {
            double[] rt_array = new double[3];
            rt_array[0] = Math.Sqrt(y * y + z * z);
            rt_array[1] = Math.Atan2(z, y);
            rt_array[2] = x;
            return rt_array;
        }
        public void DisplayBoundingBox(List<double[]> pointList)
        {
            //连线：0,4 1,5 2,6 3,7
            for (int i = 0; i < 4; i++)
            {
                CreateLineByTwoPoint(pointList[i], pointList[i + 4]);
            }
            //连线：0,3  4,7
            for (int i = 0; i < 5; i += 4)
            {
                CreateLineByTwoPoint(pointList[i], pointList[i + 3]);
            }
            //连线：0,1 1.2 2,3 
            for (int i = 0; i < 3; i++)
            {
                CreateLineByTwoPoint(pointList[i], pointList[i + 1]);
            }
            //连线：4,5 5,6 6,7
            for (int i = 4; i < 7; i++)
            {
                CreateLineByTwoPoint(pointList[i], pointList[i + 1]);
            }
        }
        public void CreateLineByTwoPoint(double[] point1, double[] point2)
        {
            Session theSession = Session.GetSession();
            UFSession theUFSession = UFSession.GetUFSession();
            Part workPart = theSession.Parts.Work;
            Point3d startPoint = UGMath.convertArrayToPoint3d(point1);
            Point3d endPoint = UGMath.convertArrayToPoint3d(point2);
            Line lineData = workPart.Curves.CreateLine(startPoint, endPoint);
            lineData.SetVisibility(SmartObject.VisibilityOption.Visible);
            theUFSession.Obj.SetColor(lineData.Tag, 186);
        }

    }

    public class PBI
    {
        public bool pbi(box obb, pipeS pipes)
        {


            List<double[]> obbVerticesCoord = new List<double[]>();
            obbVerticesCoord = obb.coordpoints;
           
            double[] startPointCoord = pipes.p1;

            double[] endPointCoord = pipes.p2;

            //尽可能找到所有分离轴，运用分离轴定律进行判断
            List<Vector3> projAxis = new List<Vector3>();//储存所有的分离轴 线段本身1条+OBB的3条

            //线段本身作为分离轴
            Vector3 lineDirection = new Vector3(endPointCoord[0] - startPointCoord[0], endPointCoord[1] - startPointCoord[1], endPointCoord[2] - startPointCoord[2]);
            lineDirection.normalize();
            projAxis.Add(lineDirection);

            //找到OBB的三个方向，作为分离轴
            for (int i = 0; i < 9; i += 3)
            {
                Vector3 tmpProjAxis = new Vector3(obb.csy.data[i], obb.csy.data[i + 1], obb.csy.data[i + 2]);
                projAxis.Add(tmpProjAxis);
            }

            //循环计算Line和obb直线边的法向量 共发现3条
            for (int i = 1; i < 4; i++)
            {
                Vector3 obbCrossLineProjAxis = new Vector3();
                Vector3.cross(obbCrossLineProjAxis, projAxis[0], projAxis[i]);
                obbCrossLineProjAxis.normalize();
                projAxis.Add(obbCrossLineProjAxis);
            }


            //储存计算值
            double min1 = 0.0, min2 = 0.0, max1 = 0.0, max2 = 0.0, dot1 = 0.0, dot2 = 0.0;
            //在每个分离轴上进行投影计算
            for (int i = 0; i < projAxis.Count; i++)
            {
                min1 = Dot(startPointCoord, projAxis[i].ToDouble());
                max1 = min1;
                //对Line另一个点进行投影
                dot1 = Dot(endPointCoord, projAxis[i].ToDouble());

                if (dot1 < min1) { min1 = dot1; }
                if (dot1 > max1) { max1 = dot1; }

                min2 = Dot(obbVerticesCoord[0], projAxis[i].ToDouble());
                max2 = min2;
                //对obb的每个点进行投影
                for (int j = 0; j < obbVerticesCoord.Count; j++)
                {
                    dot2 = Dot(obbVerticesCoord[j], projAxis[i].ToDouble());

                    if (dot2 < min2) { min2 = dot2; }
                    if (dot2 > max2) { max2 = dot2; }
                }
                //UGTools.writeInListWindow(octreeNode.nodeLabel.ToString() + "," + min2.ToString() + "," + max2.ToString() + "," + min1.ToString() + "," + max1.ToString());
                //判断是否Line和节点在该分离轴上投影相交
                //if (max1 + pipes.d < min2 || max2 < min1- pipes.d)
                if (max1 < min2 || max2 < min1 )
                {
                    return false;    //分离轴上投影不相交，未发生碰撞 
                }

            }

            return true;
        
        }
     

        public double Dot(double[] p, double[] q)
        {
            if (p.Length != q.Length)
            {
                throw new ArgumentException("向量维度不对");
            }

            return p.Zip(q, (x, y) => x * y).Sum();
        }
        public double[] Sub(double[] p, double[] q)
        {
            if (p.Length != q.Length)
            {
                throw new ArgumentException("向量维度不对");
            }

            double[] result = new double[p.Length];
            for (int i = 0; i < p.Length; i++)
            {
                result[i] = p[i] - q[i];
            }
            return result;
        }
    }


}
